% Fast Linear Quaternion Attitude Estimator from Vector Observations
% Published in IEEE Transactions on Automation Science and Engineering
% Authors: Jin Wu, Zebo Zhou et al.
% Copytight (c) 2016

function Q = FLAE(r_base, b_base, weights, method)

    MM = zeros(3, 3);
   
    for i = 1 : length(b_base(1, :))
        MM = MM + weights(i) * b_base(:, i) * r_base(:, i)';
    end

    hx = MM(:, 1)';
    hy = MM(:, 2)';
    hz = MM(:, 3)';
    
    % H1_matrix
    Hx1_matrix = hx(1);  Hy1_matrix = hx(2);  Hz1_matrix = hx(3);
    H1_matrix = [
        Hx1_matrix,   0, -Hz1_matrix, Hy1_matrix;
          0, Hx1_matrix,  Hy1_matrix, Hz1_matrix;
       -Hz1_matrix, Hy1_matrix, -Hx1_matrix,   0;
        Hy1_matrix, Hz1_matrix,    0,-Hx1_matrix];
    
    % H2_matrix
    Hx2_matrix = hy(1);  Hy2_matrix = hy(2);  Hz2_matrix = hy(3);
    H2_matrix = [
        Hy2_matrix,  Hz2_matrix,    0, -Hx2_matrix;
        Hz2_matrix, -Hy2_matrix,  Hx2_matrix,    0;
          0,  Hx2_matrix,  Hy2_matrix,  Hz2_matrix;
       -Hx2_matrix,    0,  Hz2_matrix, -Hy2_matrix];
   
   % H3_matrix
    Hx3_matrix = hz(1);  Hy3_matrix = hz(2);  Hz3_matrix = hz(3);
    H3_matrix = [
        Hz3_matrix, -Hy3_matrix,  Hx3_matrix,    0;
       -Hy3_matrix, -Hz3_matrix,    0,  Hx3_matrix;
        Hx3_matrix,    0, -Hz3_matrix,  Hy3_matrix;
          0,  Hx3_matrix,  Hy3_matrix,  Hz3_matrix];
    
    WW = H1_matrix + H2_matrix + H3_matrix;
    
    switch(int32(method))
        case int32(0)
            % method name 'symbolic'
            c = det(WW);
            b = - 8 * det(MM');
            a = - 2 * trace(MM * MM');

            T0 = 2 * a^3 + 27 * b^2 - 72 * a * c;
            T1 = (T0 + sqrt(complex(- 4 * (a^2 + 12 * c)^3 + T0^2)))^(1 / 3);
            T2 = sqrt(- 4 * a + 2^(4 / 3) * (a^2 + 12 * c) / T1 + 2^(2 / 3) * T1);
   
            lambda1 =   0.20412414523193150818310700622549 * (T2 - sqrt(-T2^2 - 12 * a - 12 * 2.4494897427831780981972840747059 * b / T2));
            lambda2 =   0.20412414523193150818310700622549 * (T2 + sqrt(-T2^2 - 12 * a - 12 * 2.4494897427831780981972840747059 * b / T2));
            lambda3 = - 0.20412414523193150818310700622549 * (T2 + sqrt(-T2^2 - 12 * a + 12 * 2.4494897427831780981972840747059 * b / T2));
            lambda4 = - 0.20412414523193150818310700622549 * (T2 - sqrt(-T2^2 - 12 * a + 12 * 2.4494897427831780981972840747059 * b / T2));
    
            lambda = max(real([lambda1 lambda2 lambda3 lambda4]));
    
            G = WW - lambda * eye(4);

            pivot = G(1, 1);
            G(1, :) = G(1, :) / pivot;
            G(2, :) = G(2, :) - G(2, 1) * G(1, :);
            G(3, :) = G(3, :) - G(3, 1) * G(1, :);
            G(4, :) = G(4, :) - G(4, 1) * G(1, :);

   
            pivot = G(2, 2);
            G(2, :) = G(2, :) / pivot;
            G(1, :) = G(1, :) - G(1, 2) * G(2, :);
            G(3, :) = G(3, :) - G(3, 2) * G(2, :);
            G(4, :) = G(4, :) - G(4, 2) * G(2, :);
   
            pivot = G(3, 3);
            G(3, :) = G(3, :) / pivot;
            G(1, :) = G(1, :) - G(1, 3) * G(3,:);
            G(2, :) = G(2, :) - G(2, 3) * G(3,:);
            G(4, :) = G(4, :) - G(4, 3) * G(3,:);
    
            q = [G(1, 4); G(2, 4); G(3, 4); -1];
            
            % return the result Q
            Q = q ./ norm(q);
        
        case int32(1)
            % method name 'eig'
            [V, ~] = eig(WW);
            q = V(:, 4);
            
            % return the result Q
            Q = q ./ norm(q);
            
        otherwise 
            % method name is 'newtown'
            c = det(WW);
            b = - 8 * det(MM');
            a = - 2 * trace(MM * MM');
        
            lambda = 1.0;
            old_lambda = 1.0;
            iter = 0;
 
            while(abs(old_lambda - lambda) > 1e-8 && iter <= 50)
                old_lambda = lambda;
                lambda = lambda - ((lambda^2 + a / 2)^2 + b * lambda + c - a^2 / 4) / ...
                            (4 * lambda * (lambda^2 + a / 2) + b);
                iter = iter + 1;
            end
        
            G = WW - lambda * eye(4);

            pivot = G(1, 1);
            G(1, :) = G(1, :) / pivot;
            G(2, :) = G(2, :) - G(2, 1) * G(1, :);
            G(3, :) = G(3, :) - G(3, 1) * G(1, :);
            G(4, :) = G(4, :) - G(4, 1) * G(1, :);

   
            pivot = G(2, 2);
            G(2, :) = G(2, :) / pivot;
            G(1, :) = G(1, :) - G(1, 2) * G(2, :);
            G(3, :) = G(3, :) - G(3, 2) * G(2, :);
            G(4, :) = G(4, :) - G(4, 3) * G(2, :);
   
            pivot = G(3, 3);
            G(3, :) = G(3, :) / pivot;
            G(1, :) = G(1, :) - G(1, 3) * G(3,:);
            G(2, :) = G(2, :) - G(2, 3) * G(3,:);
            G(4, :) = G(4, :) - G(4, 3) * G(3,:);
    
            q = [G(1, 4); G(2, 4); G(3, 4); -1];
            
            % return the result Q
            Q = q ./ norm(q);
        
    end
    
    disp(Q)
    
end

